#ifndef SOURCESTENCIL_IMPL_H
#define SOURCESTENCIL_IMPL_H

#include <limits>
#include <exception>
#include <sstream>
#include <algorithm>
#include "Core/VecCompare.h"
#include "Embedded/SourceStencil.h"
#include "Embedded/PLG.h"

template <int Dim>
template <class T_DiffOp>
SourceStencil<Dim>::SourceStencil(const EmGrid &aGrid,
                                  const T_DiffOp &diffOp,
                                  const SourceStencilParam<Dim> &param)
    : emGrid(aGrid)
{
  const int n = T_DiffOp::fitOrder - 1;
//  const Box<Dim> valid = emGrid;
  const bool useOuterRect = emGrid.useOuterRect;
  const auto &nodeLabels = emGrid.nodeLabels;
  const auto &outNormals = emGrid.outNormals;
  const auto &associatedGhost = emGrid.associatedGhost;
  using NodeType = typename EmGrid::NodeType;
  using OperatorType = typename SourceStencilParam<Dim>::OperatorType;
  ddfor(emGrid, [&](const iVec &idx) {
    if(nodeLabels(idx) < 0)
      return;
    if(diffOp.checkStandardScheme(nodeLabels, idx, useOuterRect))
      return;
    Stencil thisStencil;
    thisStencil.destIdx = idx;
    switch(param.otype) {
    case OperatorType::Elliptic:
    {
      bool success = false;
      for (int m = 0; m <= n/2; ++m) {
        success |= generateLatticeOnRing<n>(idx, m, idx, thisStencil.locations);
        if(success)
          break;
      }
      // =========================================
      // Modify the stencil by adding the neighboring nodes
      Box<Dim> box(idx, idx);
      box = box.inflate(n/2) & emGrid;
      ddfor(box, [&](const iVec &jdx) {
        if(norm(jdx - idx, 1) <= 2 && nodeLabels(jdx) >= 0)
          thisStencil.locations.push_back(jdx);
      }, false);
      // remove the redundant nodes
      VecCompare<int, Dim> vcmp;
      auto &locations = thisStencil.locations;
      std::sort(locations.begin(), locations.end(), vcmp);
      locations.erase(std::unique(locations.begin(), locations.end(),
                  [&](const auto &lhs, const auto &rhs) { return vcmp.compare(lhs, rhs) == 0; }),
                  locations.end());
      // =========================================
      if(!success) {
        std::ostringstream oss;
        oss << "Unable to find stencil at " << idx << ". ";
        throw std::runtime_error(oss.str());
      }
      // 3. Fill in other information
      for(const auto &jdx : thisStencil.locations) {
        thisStencil.isBdry.push_back(0);
        thisStencil.weights.push_back(std::max(norm((jdx - idx) * 1.0, 2), 0.5));
      }
      if(nodeLabels(idx) == NodeType::Bdry) {
        thisStencil.locations.push_back(associatedGhost(idx));
        thisStencil.isBdry.push_back(1);
        thisStencil.weights.push_back(0.5);
      }
    }
      break;
    case OperatorType::Convection:
    {
      // get the upwind direction
      rVec velDir;
      for (int d = 0; d < Dim; ++d)
        velDir[d] = (*param.velField)(enlarge(idx, d));
      iVec upwindLoc = static_cast<iVec>(idx - normalize(velDir) * (n/2) + 0.5);
      iVec inwardLoc = static_cast<iVec>(idx - outNormals(idx) * (n/2) + 0.5);
      const int numAttempts = 8;
      bool success = false;
      for(int k = 0; k <= numAttempts-1; ++k) {
        Real t = 1.0 * k / (numAttempts-1);
        iVec boxCenter = static_cast<iVec>(upwindLoc * (1-t) + inwardLoc * t);
        success |= generateLatticeOnRing<n>(boxCenter, 0, idx, thisStencil.locations);
        if(success)
          break;
      }
      if(!success) {
        std::ostringstream oss;
        oss << "Unable to find stencil at " << idx << ". ";
        throw std::runtime_error(oss.str());
      }
      // 3. Fill in other information.
      for(const auto &jdx : thisStencil.locations) {
        thisStencil.isBdry.push_back(0);
        thisStencil.weights.push_back(std::max(norm((jdx - idx) * 1.0, 2), 0.5));
      }
      if(nodeLabels(idx) == NodeType::Bdry) {
        thisStencil.locations.push_back(associatedGhost(idx));
        thisStencil.isBdry.push_back(1);
        thisStencil.weights.push_back(0.5);
      }
    }
      break;
    case OperatorType::Others:
      assert(!"To be implemented. ");
      break;
    default:
      assert(0);
    }
    vStencil.push_back(thisStencil);
  }, false);
}

template <int Dim>
template <int n>
bool SourceStencil<Dim>::generateLatticeOnRing(const iVec &p, int m, const iVec &q0, std::vector<iVec> &outLoc) const
{
  const auto &nodeLabels = emGrid.nodeLabels;
  // 1. Find the box of the center c of the K-box s.t. norm(c-p, 0) < m+1.
  Box<Dim> kbc;
  if(n % 2 == 0)
    kbc = Box<Dim>(p, p);
  else
    kbc = Box<Dim>(p-1, p);
  kbc = kbc.inflate(m);
  // 2. Invoke the PLG
  PLG<Dim,n>::initialize();
  Box<Dim> valid = emGrid;
  Real minCpt = std::numeric_limits<Real>::max();
  ddfor(kbc, [&](const iVec &ci) {
    if((n % 2 == 0 && norm(p-ci, 0) < m)
        || (n % 2 == 1 && norm(p-(ci + 0.5), 0) < m*1.0))
      return;
    // 1. Setup the PLG problem.
    iVec loCorner = ci - n/2;
    Box<Dim> completeBox(0, n);
    Box<Dim> validBox = completeBox & (valid - loCorner);
    Tensor<int, Dim> K(completeBox);
    K = 0;
    ddfor(validBox, [&](const iVec &jdx) {
      K(jdx) = nodeLabels(loCorner + jdx) >= 0;
    }, false);
    dbgcout3 << K << std::endl;
    iVec q = q0 - loCorner;
    if(!completeBox.contain(q) || K(q) == 0)
      return;
    // 2. Invoke the PLG algorithm.
    bool earlyReject;
    Real thisCpt;
    auto testOrder = PLG<Dim, n>::generateTestOrder(K, q * 1.0, PLG<Dim, n>::TestOrder::oneNormFirst, earlyReject);
    if(earlyReject)
      return;
    auto sol2PLG = PLG<Dim, n>::generate(K, q, q * 1.0, testOrder, thisCpt, 1);
    if(thisCpt >= 0 && thisCpt < minCpt) {
      minCpt = thisCpt;
      outLoc = PLG<Dim, n>::form(sol2PLG);
      for(auto &jdx : outLoc)
        jdx = jdx + loCorner;
    }
  }, false);
  if(minCpt == std::numeric_limits<Real>::max())
    return false;
  return true;
}

template <int Dim>
void SourceStencil<Dim>::dump(std::ostream &os) const
{
  int D = Dim;
  os.write((char*)&D, sizeof(int));
  int numItems = vStencil.size();
  os.write((char*)&numItems, sizeof(int));
  for(const auto &item : vStencil) {
    os.write((char*)(&(item.destIdx)), sizeof(int) * Dim);
    int numLocs = item.locations.size();
    os.write((char*)&numLocs, sizeof(int));
    os.write((char*)item.locations.data(), sizeof(int) * Dim * numLocs);
  }
}

#endif //SOURCESTENCIL_IMPL_H
